﻿using System;

namespace BOL.Maths.LinearAlgebra
{
    public static class Utils
    {
        public static double Pivot(double[][] m, double[] v, int k, Direction direction)
        {
            int length = v.Length;
            int i = k, j;
            double d = 0.0, dd;

            for (j = k; j < length; j++)
            {
                dd = Math.Abs(m[j][k]);
                if (dd > d)
                {
                    d = dd;
                    i = j;
                }
            }

            if (i > k)
            {
                Matrix<double>.Swap(m, k, i, direction);
                Vector<double>.Swap(v, k, i);
            }

            return m[k][k];
        }

        public static double Pivot(MatrixDouble m, VectorDouble v, int k, Direction direction)
        {
            return Pivot(m.Elements, v.Elements, k, direction);
        }

        public static void Triangulate(double[][] m, double[] v, Direction direction)
        {
            int length = m.Length;
            double d, dd;
            int i, j, k;

            for (i = 0; i < length - 1; i++)
            {
                d = Pivot(m, v, i, direction);
                if (Math.Abs(d) < 1.0e-500)
                    throw new ArgumentException("Diagonal element is too small!");
                for (j = i + 1; j < length; j++)
                {
                    dd = m[j][i] / d;
                    for (k = i + 1; k < length; k++)
                        m[j][k] -= dd * m[i][k];
                    v[j] -= dd * v[i];
                }
            }
        }

        public static double Triangulate(MatrixDouble m, VectorDouble v, Direction direction)
        {
            return Triangulate(m.Elements, v.Elements, direction);
        }
    }
}
